#!/usr/bin/env python
PACKAGE = "stair_modeling"

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

gen.add("down_sample_points_number", int_t, 0, "remain point number of each plane after random down sample", 50, 0,
        1000)
gen.add("angle_h", double_t, 0,
        "horizontal plane: plane angle [0,angle_h);"
        "slope plane:      plane angle [angle_h,angle_v);"
        "vertical plane:   plane angle [angle_v,PI/2]",
        10, 0, 90)
gen.add("angle_v", double_t, 0,
        "horizontal plane: plane angle [0,angle_h) "
        "slope plane:      plane angle [angle_h,angle_v) "
        "vertical plane:   plane angle [angle_v,PI/2]",
        70, 0, 90)

gen.add("ground_height_th", double_t, 0, "ground height threshold (unit: m)", 0.06, 0, 0.15)
gen.add("min_ground_plane_points_num", int_t, 0,
        "min point number of groud plane, this must be changed based on plane segmentation method(pcl_seg:~3500,peac:~50000)",
        50000, 0, 75000)

gen.add("merge_dis_th", double_t, 0, "the distance threshold to merge two planes (unit: m)", 0.02, 0, 0.10)
gen.add("merge_angle_th", double_t, 0, "the angle threshold to merge two planes (unit: deg)", 12, 0, 90)
gen.add("merge_distance_th", double_t, 0, "the distance threshold to merge two planes (unit: m)", 0.8, 0, 1.0)

gen.add("a1D_th", double_t, 0,
        "the threshold of a1D in paper DIMENSIONALITY BASED SCALE SELECTION IN 3D LIDAR POINT CLOUDS;"
        "which define the line like of a set of points (0~1)",
        0.85, 0, 1.0)

gen.add("decay_rate", double_t, 0, "history info decay rate", 0.5, 0, 1.0)

gen.add("vertical_plane_normal_angle_th", double_t, 0, "vertical plane normal cluster angle threshold", 10, 0, 90)
gen.add("vertical_plane_normal_est_angle_th", double_t, 0,
        "the vertical_plane_normal should be close to eigen_vector[1] of stair horizontal plane in vertical_plane_normal_est_angle_th",
        5, 0, 45)

gen.add("cv_angle_horizontal_th", double_t, 0,
        "center vector direction angle with horizontal_plane_direction in 90 degrees threshold;"
        "the center vector direction should be perpendicular with horizontal_plane_direction",
        10, 0, 90)

gen.add("cv_dir_cluster_angle_th", double_t, 0, "cluster main center vector angle threshold", 10, 0, 45)

gen.add("plane_min_length", double_t, 0, "plane length constraint", 0.4, 0, 1.0)
gen.add("plane_max_length", double_t, 0, "plane length constraint", 1.4, 0, 5.0)
gen.add("plane_min_width", double_t, 0, "plane width constraint", 0.18, 0, 0.5)
gen.add("plane_max_width", double_t, 0, "plane width constraint", 0.5, 0, 1.0)
gen.add("stair_max_height", double_t, 0, "stair height constraint", 0.5, 0, 0.8)
gen.add("stair_max_width", double_t, 0, "stair width constraint", 0.33, 0, 1.0)

gen.add("stair_plane_max_distance", double_t, 0,
        "the max distance of two planes in a stair, exceed this distance, the two plane will not be judge as in a stair",
        0.4, 0, 1)
gen.add("stair_cv_angle_diff_th", double_t, 0,
        "the max angle difference threshold of center vectors of two neighbor planes in stair when modeling",
        30, 0, 90)

gen.add("show_key_directions", bool_t, 0, "visualization settings", True)
gen.add("show_plane_info", bool_t, 0, "visualization settings", True)
gen.add("show_cloud", bool_t, 0, "visualization settings", True)
gen.add("show_stair_cloud", bool_t, 0, "visualization settings", True)
gen.add("show_downsampled_cloud", bool_t, 0, "visualization settings", False)
gen.add("show_counter", bool_t, 0, "visualization settings", True)
gen.add("show_center", bool_t, 0, "visualization settings", True)
gen.add("show_detial_model", bool_t, 0, "show detialed stair model in the pcl viewer", False)
gen.add("show_est_param", bool_t, 0, "show estimated stair paramters in the pcl viewer", False)
gen.add("show_run_time", bool_t, 0, "show run time", False)

PolygonShowModeType = gen.enum([gen.const("Line", int_t, 0, "Line Mode"),
                                gen.const("Surface", int_t, 1, "Surface Mode")],
                               "PolygonShowModeType")
gen.add("polygonShowMode", int_t, 0, "Select PolygonShowMode", 0, 0, 1, edit_method=PolygonShowModeType)

gen.add("time_window",double_t,0,"slide average filter time window size", 1, 0.1,5)

exit(gen.generate(PACKAGE, "stair_modeling_node", "stair_modeling_param"))
